#include "ros/ros.h"
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
//使用Action通信所需要的头文件

using namespace std;

typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;

int main(int argc, char *argv[])
{
    ros::init(argc, argv, "MoveBaseAction");
    ros::NodeHandle n;

    // tell the action client that we want to spin a thread by default
    // don't need ros::spin() 创建action客户端，参数1：action名，参数2：true，不需要手动调用ros::spin()，会在它的线程中自动调用。
    MoveBaseClient ac("move_base", true);

    // wait for the action server to come up1000
    while (!ac.waitForServer(ros::Duration(5.0)))
    {
        ROS_INFO("Waiting for the move_base action server to come up");
    }

    //初始化一个坐标
    geometry_msgs::PoseStamped send_Pose;
    move_base_msgs::MoveBaseGoal goal;

    send_Pose.pose.position.x = 6.0f;
    send_Pose.pose.position.y = -2.0f;
    send_Pose.pose.position.z = 0;
    send_Pose.pose.orientation.x = 0;
    send_Pose.pose.orientation.y = 0;
    send_Pose.pose.orientation.z = 0;
    send_Pose.pose.orientation.w = 1.0;

    goal.target_pose.header.frame_id = "map";
    goal.target_pose.header.stamp = ros::Time::now();

    goal.target_pose = send_Pose; 

    ac.sendGoal(goal);

    ac.waitForResult();
    if (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
    {
        ROS_INFO("already reached goal!");
        // TODO: 成功到达目的地，此处发挥想象做点啥
    }
    else
    {
        ROS_INFO("Failed to reach the goal!");
    }

    /*ros::Rate loop_rate(10);
    ros::spin();*/

    return 0;
}
